//
// Created by ZhaoXiaoFei on 2022/7/19.
//

#ifndef VINS_ESTIMATOR_INITIA_EX_RBC_HPP
#define VINS_ESTIMATOR_INITIA_EX_RBC_HPP
#include <Eigen/Dense>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <glog/logging.h>


class IninialExRbc{
public:

    IninialExRbc(){
        frame_count = 0;
        R_c.clear();
        R_imu.clear();
        tempR.clear();
        R_bc = Eigen::Matrix3d::Identity();
    }
    bool CalibrationExRbc(std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>& corres, const Eigen::Matrix3d& Rbb, Eigen::Matrix3d& res_Rbc){
        frame_count++;

        R_c.push_back(computeRelativeR(corres));
        R_imu.push_back(Rbb);
        tempR.push_back(R_bc.transpose() * Rbb * R_bc);

        Eigen::MatrixXd A(4 * frame_count, 4);
        A.setZero();

        for(int i = 0; i < frame_count; i++){
            Eigen::Quaterniond r1(R_c[i]);
            Eigen::Quaterniond r2(tempR[i]);
            double angule_distance = 180 / M_PI * r1.angularDistance(r2);

            double huber = angule_distance > 5.0 ? 5.0 / angule_distance : 1.0;

            Eigen::Matrix4d L, R;
            L = Utility::Qleft(Eigen::Quaterniond(R_c[i]));
            R = Utility::Qright(Eigen::Quaterniond(R_imu[i]));

            A.block<4,4>(4 * i, 0) = huber * (L - R);
        }

        Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
        Eigen::Matrix<double, 4, 1> temp = svd.matrixV().col(3);
        //Eigen::Matrix<double, 4, 1> x(temp(1), temp(2), temp(3), temp(0));
        Eigen::Quaterniond rbc(temp(0), temp(1), temp(2), temp(3));
//        Eigen::Quaterniond rbc(temp);
//        auto n = svd.singularValues().col(3);
        LOG(INFO) << " svd.singularValues().transpose(): " << svd.singularValues().transpose() << std::endl;
        Eigen::Vector3d ric_cov;
        ric_cov = svd.singularValues().tail<3>();
        LOG(INFO) << " ric_cov.transpose(): " << ric_cov.transpose() << std::endl;
        if(frame_count >= 10 && ric_cov(1) > 0.243){
            res_Rbc = rbc.toRotationMatrix().transpose();
//            for(int i = 0; i < R_c.size(); i++){
//                LOG(INFO) << i << " | " << Eigen::Quaterniond(R_c[i]) << std::endl;
//            }
            return true;
        }
        else{
            return false;
        }
        return false;
    }

    Eigen::Matrix3d computeRelativeR(std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>& corres){
        if(corres.size() >= 9){
            std::vector<cv::Point2f> a, b;
            for(auto& it: corres){
                a.push_back(cv::Point2f(it.first.x() / it.first.z(), it.first.y() / it.first.z()));
                b.push_back(cv::Point2f(it.second.x() / it.second.z(), it.second.y() / it.second.z()));
            }
            cv::Mat E = cv::findFundamentalMat(a, b);
            cv::Mat cameraMatrix = cv::Mat::eye(3, 3, CV_64F);
            cv::Mat R, t, mask;
            recoverPose(E, a, b, cameraMatrix, R, t, mask);
            //LOG(INFO) << "R: " << R << "t: " << t << std::endl;
            Eigen::Matrix3d res;
            cv::cv2eigen(R, res);
            return res.transpose();
        }
        return Eigen::Matrix3d::Identity();
    }

    int frame_count;
    std::vector<Eigen::Matrix3d> R_c, R_imu, tempR;
    Eigen::Matrix3d R_bc;
};

#endif //VINS_ESTIMATOR_INITIA_EX_RBC_HPP
